﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.IO;
using System.Text;
using UnityEngine;

namespace PathFind.Algo {
    public class AStarJps : MonoBehaviour, IaStarAlgo {
        private BinaryHeap<Node> _openListBh;
        private NodePool<Node> _pool;

        private BitArray GridData { get; set; }
        private BitArray TestedGrid { get; set; }
        public short GridHeight { get; private set;}
        public short GridWidth { get; private set; }

        private Coordinate startPos;
        private Coordinate endPos;
        private int startIndex;
        private int endIndex;
        private bool preprocessedData;
        private List<Coordinate> tempSol = new List<Coordinate>();

        #region 初始化
        public AStarJps() {
        }
        public AStarJps(short width, short height) {
            ReInitData(width, height);
        }

        public AStarJps(BitArray grid, short width, short height) {
            ReInitData(grid, width, height);
        }

        public void ReInitData(short width, short height) {
            var bitArray = new BitArray(width * height, true);
            ReInitData(bitArray, width, height);
        }
        public void ReInitData(BitArray gd, short width, short height) {
            _openListBh = new BinaryHeap<Node>();
            _pool = new NodePool<Node>();
            if (gd.Count < width * height) {
                throw new Exception("grid.Count lesser than width*height");
            }

            GridData = gd;
            GridWidth = width;
            GridHeight = height;
            TestedGrid = new BitArray(GridData.Count);
        }
        #endregion

        #region 格子检查方法
        public short GetHeight() {
            return GridHeight;
        }
        public short GetWidth() {
            return GridWidth;
        }
        public bool InBounds(int index) {
            return index < (GridHeight * GridWidth) && index >= 0;
        }

        public bool InBounds(Coordinate c) {
            return InBounds(GridIndex(c));
        }

        public bool InBounds(short x, short y) {
            return InBounds(GridIndex(x, y));
        }

        private int GridIndex(Coordinate c) {
            return GridIndex(c.x, c.y);
        }

        private int GridIndex(short x, short y) {
            if (x < 0 || x >= GridWidth || y < 0 || y >= GridHeight) {
                return -1;
            }

            return y * GridWidth + x;
        }

        private Coordinate IndexToCoordinate(in int index) {
            return new Coordinate((short) (index % GridWidth), (short) (index / GridWidth));
        }

        public bool IsCoordinateBlocked(Coordinate c) {
            return IsPassable(c);
        }

        public bool IsPassable(Coordinate c) {
            int index = GridIndex(c);
            return IsPassable(index);
        }

        public bool IsPassable(int index) {
            if (index == -1) {
                return false;
            }
            return GridData[index];
        }

        public bool IsPassable(short x, short y) {
            int index = GridIndex(x, y);
            return IsPassable(index);
        }

        private void SetChecked(int index) {
            TestedGrid[index] = true;
        }

        private bool IsChecked(int index) {
            return TestedGrid[index];
        }
        #endregion

        #region 算法主体
        public Coordinate[] FindSolution2(short sX, short sY, short eX, short eY) {
            FindSolution(sX, sY, eX, eY, ref tempSol);
            return tempSol.ToArray();
        }
        public bool FindSolution(short sX, short sY, short eX, short eY, ref List<Coordinate> solution) {
            endPos = new Coordinate(eX, eY);
            endIndex = GridIndex(eX, eY);
            startPos = new Coordinate(sX, sY);
            startIndex = GridIndex(sX, sY);
            if (sX == eX && sY == eY) {
                return false;
            }
            if (!InBounds(startIndex) || !InBounds(endIndex) || !IsPassable(startPos) || !IsPassable(endPos)) {
                return false;
            }
            _openListBh.Clear();
            TestedGrid.SetAll(false);
            _pool.Reset();
            solution.Clear();
            var startNode = _pool.GetNewNode(startPos, endPos, Direction.CENTER, null);
            _openListBh.Insert(startNode);
            SetChecked(GridIndex(startPos));
            DirectionSet dSet = new DirectionSet();
            while (_openListBh.Count() > 0) {
                Node currentNode = _openListBh.PopMin();
                dSet.Clear();
                AddForcedNeighbours(ref dSet, ref currentNode.pos, currentNode.dir);
                AddNaturalNeighbours(ref dSet, currentNode.dir);
                for (Direction dir = Direction.START; dir < Direction.END; dir++) {
                    if (!dSet.IsDirectionSet(dir)) {
                        continue;
                    }
                    int nextIndex = Jump(currentNode.pos, dir);
                    if (!InBounds(nextIndex)) {
                        continue;
                    }
                    Coordinate nc = IndexToCoordinate(nextIndex);
                    if (nextIndex == endIndex) {
                        solution.Add(endPos);
                        Node solutionNode = currentNode;
                        while (solutionNode != null) {
                            solution.Add(solutionNode.pos);
                            solutionNode = solutionNode.parent;
                        }
                        _openListBh.Clear();
                        return true;
                    }

                    if (!IsChecked(nextIndex)) {
                        var newNode = _pool.GetNewNode(nc, endPos, dir, currentNode);
                        _openListBh.Insert(newNode);
                        SetChecked(nextIndex);
                    }
                    else {
                        Node t = _pool.GetNewNode(nc, endPos, dir, currentNode);
                        _openListBh.InsertSmaller(t);
                    }
                }
            }
            return false;
        }
        #endregion

        #region 跳点查找
        private int Jump(Coordinate currentNodePos, Direction dir) {
            Coordinate nc = currentNodePos.AddDirOffset(dir);
#if DIAG_UNBLOCKED
            if (dir.IsDiag()) {
                Coordinate nextC2 = currentNodePos.AddDirPreviousOffset(dir);
                Coordinate nextC3 = currentNodePos.AddDirNextOffset(dir);
                var canThroughDiag = InBounds(nextC2) && InBounds(nextC3) && IsPassable(nextC2)
                                 && IsPassable(nextC3);
                if (!canThroughDiag) {
                    return -1;
                }
            }
#endif
            while (true) {
#if DEBUG_GRID_SHOW
                if (nc != startPos && nc != endPos && InBounds(nc) && IsPassable(nc)) {
                    grid.ChangeColor(nc, Color.cyan);
                }
#endif
                if (!IsPassable(nc)) {
                    return -1;
                }

                int index = GridIndex(nc);
                if (index == endIndex || HasForcedNeighbours(ref nc, dir)) {
                    return index;
                }

                if (dir.IsDiag()) {
                    int next = Jump(nc, dir.AddRoundStep(7));
                    if (next >= 0) {
                        return index;
                    }

                    next = Jump(nc, dir.AddRoundStep(1));
                    if (next >= 0) {
                        return index;
                    }
                }
                nc = nc.AddDirOffset(dir);
            }
        }

        bool CanEnter(Coordinate co, Direction dir, short roundStep) {
            return IsPassable(co.AddDirPlusRoundStep(dir, roundStep));
        }
        #endregion

        #region 邻居
        bool Implies(bool a, bool b) {
            return !a || b;
        }
        // 看 《Online Graph Pruning for Pathfnding on Grid Maps》 Figure2
        public void AddForcedNeighbours(ref DirectionSet neighSet, ref Coordinate pos, Direction dir) {
            if (dir >= Direction.CENTER) {
                return;
            }
            if (dir.IsDiag()) {
#if DIAG_UNBLOCKED
                // 是否需要加 CanEnter(pos, dir, 7) 这条件,要看对角线可走的定义:
                //     这里采用的是dir+1和dir-1至少有一个点可走时,dir才是可走的
                if (CanEnter(pos, dir, 6)
                    && (!CanEnter(pos, dir, 5) && CanEnter(pos, dir, 7))) {
                    neighSet.AddDirectionToSet(dir.AddRoundStep(6));
                }
                if (CanEnter(pos, dir, 2) &&
                    (!CanEnter(pos, dir, 3) && CanEnter(pos, dir, 1))) {
                    neighSet.AddDirectionToSet(dir.AddRoundStep(2));
                }
#else
                if (!Implies(CanEnter(pos, dir, 6) ,CanEnter(pos, dir, 5)))
                    neighSet.AddDirectionToSet(dir.AddRoundStep(6));
                if (!Implies(CanEnter(pos, dir, 2) ,CanEnter(pos, dir, 3)))
                    neighSet.AddDirectionToSet(dir.AddRoundStep(2));
#endif
            }
            else {
#if DIAG_UNBLOCKED
                // 2 可走 3 不可走的情况下
                // 这里采用这种格式是为了跟C++代码一致
                if (!Implies(CanEnter(pos, dir, 2), CanEnter(pos, dir, 3))) {
                    neighSet.AddDirectionToSet(dir.AddRoundStep(2));
                    if (CanEnter(pos, dir, 1)) {
                        neighSet.AddDirectionToSet(dir.AddRoundStep(1));
                    }
                }
                // 6 可走 5 不可走的情况下
                if (!Implies(CanEnter(pos, dir, 6), CanEnter(pos, dir, 5))) {
                    neighSet.AddDirectionToSet(dir.AddRoundStep(6));
                    if (CanEnter(pos, dir, 7)) {
                        neighSet.AddDirectionToSet(dir.AddRoundStep(7));
                    }
                }
#else
                if (!Implies(CanEnter(pos, dir, 7) ,CanEnter(pos, dir, 6)))
                    neighSet.AddDirectionToSet(dir.AddRoundStep(7));
                if (!Implies(CanEnter(pos, dir, 1) ,CanEnter(pos, dir, 2)))
                    neighSet.AddDirectionToSet(dir.AddRoundStep(1));
#endif
            }
        }
        public bool HasForcedNeighbours(ref Coordinate pos, Direction dir) {
            if (dir >= Direction.CENTER) {
                return true;
            }
            if (dir.IsDiag()) {
#if DIAG_UNBLOCKED
                if (CanEnter(pos, dir, 6)
                    && (!CanEnter(pos, dir, 5) && CanEnter(pos, dir, 7))) {
                    return true;
                }
                if (CanEnter(pos, dir, 2) &&
                    (!CanEnter(pos, dir, 3) && CanEnter(pos, dir, 1))) {
                    return true;
                }
#else
                if (!Implies(CanEnter(pos, dir, 6), CanEnter(pos, dir, 5)))
                    return true;
                if (!Implies(CanEnter(pos, dir, 2) ,CanEnter(pos, dir, 3)))
                    return true;
#endif
            }
            else {
#if DIAG_UNBLOCKED
                // 2 可走 3 不可走的情况下
                // 这里采用这种格式是为了跟C++代码一致
                if (!Implies(CanEnter(pos, dir, 2), CanEnter(pos, dir, 3))) {
                    return true;
                }
                // 6 可走 5 不可走的情况下
                if (!Implies(CanEnter(pos, dir, 6), CanEnter(pos, dir, 5))) {
                    return true;
                }
#else
                if (!Implies(CanEnter(pos, dir, 7) ,CanEnter(pos, dir, 6)))
                    return true;
                if (!Implies(CanEnter(pos, dir, 1) ,CanEnter(pos, dir, 2)))
                    return true;
#endif
            }
            return false;
        }
        public void AddNaturalNeighbours(ref DirectionSet neighSet, Direction dir) {
            if (dir >= Direction.CENTER) { // 初始点的八个方向都是自然邻居
                neighSet.SetAll(true);
                return;
            }
            // 水平方向自然邻居,参考Figure2(a)中白色的点
            // 垂直方向自然邻居,参考Figure2(c)中白色的点
            // 后续会判断点是否可走,就不在这里判断
            neighSet.AddDirectionToSet(dir);
            if (dir.IsDiag()) {
                neighSet.AddDirectionToSet(dir.AddRoundStep(1));
                neighSet.AddDirectionToSet(dir.AddRoundStep(7));
            }
        }
        #endregion

        #region 数据处理相关等方法
        public void PreProcessGrid() {
            preprocessedData = true;
        }

        public int MemorySize() {
            return -1; // 返回 -1 表示没有实现
        }
        public bool HasPreprocessedMap() {
            return preprocessedData;
        }
        public void ReProcessGrid(short lx, short rx, short ty, short by) {
        }

        private static short version = 1;
        public void WriteDataToFile(BinaryWriter fout) {
            fout.Write(version);
            fout.Write(GridWidth);
            fout.Write(GridHeight);
            byte[] bytes = GridData.ToByteArray(GridHeight, GridWidth);
            fout.Write(bytes);
        }
        public void WritePreDataToFile(BinaryWriter fout) {
        }
        public void ReadDataFromFile(BinaryReader fin) {
            fin.ReadInt16(); // version
            short width = fin.ReadInt16();
            short height = fin.ReadInt16();
            var byteLen = AStarMetrics.BitArrayLength(width, height);
            byte[] bytes = fin.ReadBytes(byteLen);
            BitArray bitArray = new BitArray(bytes);
            for (int i = width*height; i < bitArray.Count; i++) {
                bitArray[i] = false;
            }
            ReInitData(bitArray, width, height);
        }
        public void ReadPreDataFromFile(BinaryReader fin) {
        }

        public void SetCanWalkState(Coordinate co, bool canWalk) {
            var index = GridIndex(co);
            GridData[index] = canWalk;
            // grid.ChangeColor(co, GridColor.Black);
        }

        public void SetPassable(short x, short y, bool canWalk)
        {
            var index = GridIndex(x, y);
            GridData[index] = canWalk;
        }

        #endregion
#if DEBUG_GRID_SHOW
        public SquireGrid grid;
        public void SetGrid(SquireGrid squireGrid) {
            grid = squireGrid;
        }

        static readonly WaitForSeconds waitSecond = new WaitForSeconds(1f);

        #region 调试版FindSolution
        public IEnumerator FindSolution(short sX, short sY, short eX, short eY) {
            List<Coordinate> solution = new List<Coordinate>();
            endPos = new Coordinate(eX, eY);
            endIndex = GridIndex(eX, eY);
            startPos = new Coordinate(sX, sY);
            startIndex = GridIndex(sX, sY);
            if (sX == eX && sY == eY) {
                yield break;
            }
            if (!InBounds(startIndex) || !InBounds(endIndex) || !IsPassable(startPos) || !IsPassable(endPos)) {
                yield break;
            }
            _openListBh.Clear();
            TestedGrid.SetAll(false);
            _pool.Reset();
            solution.Clear();
            var startNode = _pool.GetNewNode(startPos, endPos, Direction.CENTER, null);
            _openListBh.Insert(startNode);
            SetChecked(GridIndex(startPos));
            DirectionSet dSet = new DirectionSet();
            while (_openListBh.Count() > 0) {
                Node currentNode = _openListBh.PopMin();
                if (currentNode.pos != startPos) {
                    grid.ChangeColor(currentNode.pos, Color.yellow);
                }
                yield return waitSecond;
                dSet.Clear();
                AddForcedNeighbours(ref dSet, ref currentNode.pos, currentNode.dir);
                AddNaturalNeighbours(ref dSet, currentNode.dir);
                for (Direction dir = Direction.START; dir < Direction.END; dir++) {
                    if (!dSet.IsDirectionSet(dir)) {
                        continue;
                    }
                    int nextIndex = Jump(currentNode.pos, dir);
                    if (!InBounds(nextIndex)) {
                        continue;
                    }
                    Coordinate nc = IndexToCoordinate(nextIndex);
                    // 找到终点
                    if (nextIndex == endIndex) {
                        solution.Add(endPos);
                        Node solutionNode = currentNode;
                        while (solutionNode != null) {
                            solution.Add(solutionNode.pos);
                            solutionNode = solutionNode.parent;
                        }
                        if (solution.Count > 2) {
                            for (int i = 1; i < solution.Count - 1; i++) {
                                var c = solution[i];
                                grid.ChangeColor(c, Color.green);
                            }
                        }
                        _openListBh.Clear();
                        yield break;
                    }

                    if (!IsChecked(nextIndex)) {
                        var newNode = _pool.GetNewNode(nc, endPos, dir, currentNode);
                        grid.ChangeLabel(newNode.pos, newNode.Total);
                        _openListBh.Insert(newNode);
                        SetChecked(nextIndex);
                        yield return waitSecond;
                    }
                    else {
                        Node t = _pool.GetNewNode(nc, endPos, dir, currentNode);
                        var isUpdate = _openListBh.InsertSmaller(t);
                        if (isUpdate) {
#if SHOW_GRID_TEXT
                            grid.ChangeLabel(t.pos, t.Total);
#endif
                            grid.ChangeColor(t.pos, Color.yellow);
                            yield return waitSecond;
                        }
                    }
                }
            }
        }
        #endregion
#endif
        public string DumpTxtMapData()
        {
            StringBuilder sb = new StringBuilder();
            sb.Append("type octile\n");
            sb.Append($"height {GridHeight}\n");
            sb.Append($"width {GridWidth}\n");
            sb.Append("map\n");
            for (int j = 0; j < GridHeight; j++)
            {
                for (int i = 0; i < GridWidth; i++)
                {
                    if (IsPassable((short) i, (short) j))
                    {
                        sb.Append(".");
                    }
                    else
                    {
                        sb.Append("@");
                    }
                }
                sb.Append("\n");
            }

            return sb.ToString();
        }
    }
}